﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace TimbreRecognition
{
    public class MFCC
    {
        private const double MinFreq = 27.5;  //A of the double contraoctave
        private const double MaxFreq = 4186;//C of the 5th octave
        private const double LifterExp = 0.8;

        private int numCoeffs;
        private int melBands;
        private int numFreqs;
        private int fftSize;
        private double sampleRate;

        public Matrix melWeights = null;
        public Matrix dctMat = null;
        public double[] lifterWeights;

        public MFCC(int fftSize, int numCoeffs, int melBands, double sampleRate)
	    { 
	        // Number of non-redundant frequency bins
	        numFreqs = fftSize/2 + 1;
            this.fftSize = fftSize;
	        this.numCoeffs = numCoeffs;
	        this.melBands = melBands;
	        this.sampleRate = sampleRate;
	    
            //calculate fft frequences
	        double[] fftFreqs = new double[fftSize];
	        for (int i = 0; i < fftSize; i ++)
	        {
	    	    fftFreqs[i] = (double)i/(double)fftSize*this.sampleRate;
	        }
	    
	        double minMel = HzToMel(MinFreq);
	        double maxMel = HzToMel(MaxFreq);

            double[] binFreqs = new double[melBands + 2];
	        for (int i = 0; i < melBands + 2; i ++)
	        {
                double mel = minMel + i / (melBands + 1.0) * (maxMel - minMel);
                binFreqs[i] = MelToHz(mel);
	        }

            // Precompute mel-scale auditory perceptual spectrum
            melWeights = new Matrix(melBands, fftSize, 0);
            //In debug it seems ok, but how does it work? 
	        for (int i = 0; i < melBands; i ++)
	        {
	    	    for (int j = 0; j < fftSize; j ++)
	    	    {
	    		    double lowSlope = (fftFreqs[j] - binFreqs[i])/(binFreqs[i+1] - binFreqs[i]);
		    	    double highSlope = (binFreqs[i+2] - fftFreqs[j])/(binFreqs[i+2] - binFreqs[i+1]);
		    	    melWeights.A[i][j] = Math.Max(0, Math.Min(lowSlope, highSlope));
	    	    }
	        }
	    
	        // Keep only positive frequency parts of Fourier transform
	        melWeights = melWeights.getMatrix(0, melBands - 1, 0, numFreqs - 1);
	    
	        // Precompute DCT matrix (Discrete Cosine Transformation)
	        dctMat = new Matrix(numCoeffs, melBands, 1); //initialize with ones
	        double scale = Math.Sqrt(2.0/melBands);
	        for (int i = 1; i < numCoeffs;i++)
	        {
	    	    for (int j = 0; j < melBands; j++)
	    	    {
	    		    double phase = j*2 + 1;
		    	    dctMat.A[i][j] = Math.Cos(phase*(i + 1)*Math.PI/(2.0*melBands))*scale;
	    	    }
	        }

            double root = 1.0 / Math.Sqrt(melBands);
            for (int i = 0; i < melBands; i++)
            {
                dctMat.A[0][i] *= root;
            }
	    
	        // Precompute lifting vector
	        lifterWeights = new double[numCoeffs];
	        for (int i = 0; i < numCoeffs; i ++)
	        {
	    	   // lifterWeights[i] = Math.Pow(i + 1, LifterExp);
                lifterWeights[i] = 1 + (numCoeffs / 2) * Math.Sin(Math.PI*(i) / (2 * numCoeffs));
	        }
	    }

        public double[] cepstrum(double[] re, double[] im)
        {
            Matrix powerSpec = new Matrix(numFreqs, 1);
            for (int i = 0; i < numFreqs; i++)
            {
                powerSpec.A[i][0] = (re[i] * re[i] + im[i] * im[i]) / fftSize;
            }

            // melWeights - melBands x numFreqs
            // powerSpec  - numFreqs x 1
            // melWeights*powerSpec - melBands x 1
            // aSpec      - melBands x 1
            // dctMat     - numCoeffs x melBands
            // dctMat*log(aSpec) - numCoeffs x 1

            Matrix aSpec = melWeights.times(powerSpec);
            Matrix logMelSpec = new Matrix(melBands, 1);
            for (int i = 0; i < melBands; i++)
            {
                logMelSpec.A[i][0] = Math.Log(aSpec.A[i][0]);
            }

            Matrix melCeps = dctMat.times(logMelSpec);

            double[] ceps = new double[numCoeffs];
            for (int i = 0; i < numCoeffs; i++)
            {
                ceps[i] = lifterWeights[i] * melCeps.A[i][0];
            }


            return ceps;
        }

        public double MelToHz(double mel)
        {
            return 700.0 * (Math.Exp(mel / 1125) - 1.0);
        }

        public double HzToMel(double freq)
        {
            return 1125 * Math.Log(1.0 + freq / 700.0);
        }

    }
}
